#include "CameraV4L.h"

#include <algorithm>
#include <iostream>
#include <stdlib.h>
using namespace std;

namespace movesense {

CameraV4L::CameraV4L( CameraInfo info ) : CameraBase(info)
{
	dev_name  = info.devName;
}

CameraV4L::~CameraV4L(void)
{

}

int CameraV4L::xioctl(int fd, int request, void *arg)
{
	int ret;
	do
		ret = ioctl(fd, request, arg);
	while (-1 == ret && EINTR == errno);

	if(ret < 0)
	{
		return MS_FAIL;
	}
	return MS_SUCCESS;
}

int CameraV4L::set_format(int width, int height)
{
	struct v4l2_format fmt;
	memset(&fmt, 0, sizeof(fmt));
	fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.fmt.pix.width       = width;
	fmt.fmt.pix.height      = height;
	fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;

	int ret = xioctl (fd, VIDIOC_S_FMT, &fmt);
	if(ret < 0)
	{
		return MS_FAIL;
	}

	return MS_SUCCESS;
}

int CameraV4L::request_and_mmap()
{
	int n_buffers, ret;

	struct v4l2_requestbuffers req;
	memset(&req, 0, sizeof(req));
	req.count		= REQ_BUF_LEN;
	req.type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory		= V4L2_MEMORY_MMAP;

	ret = xioctl (fd, VIDIOC_REQBUFS, &req);
	if(ret < 0)
	{
		return MS_FAIL;
	}
	
	for (n_buffers = 0; n_buffers < req.count; ++n_buffers)
	{
		struct v4l2_buffer buf;   
		memset(&buf, 0, sizeof(buf));

		buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory      = V4L2_MEMORY_MMAP;
		buf.index       = n_buffers;
		ret = xioctl(fd, VIDIOC_QUERYBUF, &buf);
		if (ret < 0)
		{
			return MS_FAIL;
		}
		buffers[n_buffers].length = buf.length;

		buffers[n_buffers].start = mmap (NULL, buf.length, PROT_READ | PROT_WRITE ,MAP_SHARED, fd, buf.m.offset); 

		if (MAP_FAILED == buffers[n_buffers].start)
		{
			return MS_FAIL;
		}
	}

	for (int i = 0; i < n_buffers; ++i)
	{
		struct v4l2_buffer buf;
		memset(&buf, 0, sizeof(buf));
		buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory      = V4L2_MEMORY_MMAP;
		buf.index       = i;

		ret = xioctl (fd, VIDIOC_QBUF, &buf);
		if (ret < 0)
		{
			return MS_FAIL;
		}
	}

	return MS_SUCCESS;
}

int CameraV4L::unmmap()
{
	for (int i = 0; i < REQ_BUF_LEN; ++i)
	{
		munmap(buffers[i].start, buffers[i].length);
	}

	return MS_SUCCESS;
}

int CameraV4L::SetSaturation(long value)
{	
	v4l2_control ctrl;
	ctrl.id 	= V4L2_CID_SATURATION;
	ctrl.value 	= value;
	return xioctl(fd, VIDIOC_S_CTRL, &ctrl);
}

int CameraV4L::GetSaturation(long *value)
{	
	v4l2_control ctrl;
	ctrl.id 	= V4L2_CID_SATURATION;
	int ret = xioctl(fd, VIDIOC_G_CTRL, &ctrl);
	*value = ctrl.value;
	return ret;
}

int CameraV4L::SetWhiteBalance(long value)
{
	v4l2_control ctrl;
	ctrl.id 	= V4L2_CID_WHITE_BALANCE_TEMPERATURE;
	ctrl.value 	= value;
	return xioctl(fd, VIDIOC_S_CTRL, &ctrl);
}

int CameraV4L::GetWhiteBalance(long *value)
{
	v4l2_control ctrl;
	ctrl.id = V4L2_CID_WHITE_BALANCE_TEMPERATURE;
	int ret = xioctl(fd, VIDIOC_G_CTRL, &ctrl);
	*value = ctrl.value;
	
	return ret;
}

int CameraV4L::Open(int width, int height)
{
	fd = open (dev_name.c_str(), O_RDWR | O_NONBLOCK, 0);

 	if (-1 == fd)
    {
		return MS_FAIL;
    }

	return set_format(width, height);
}

void CameraV4L::Close()
{
	close(fd);
}

int CameraV4L::StartStream()
{	
	int ret = request_and_mmap();
	if(ret != MS_SUCCESS)
	{
		return ret;
	}

	enum v4l2_buf_type type;
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ret = xioctl (fd, VIDIOC_STREAMON, &type);
	if (ret < 0) 
	{
		return MS_FAIL;
	}

	return MS_SUCCESS;
}

int CameraV4L::StopStream()
{
	int ret;
	enum v4l2_buf_type type;
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ret = xioctl (fd, VIDIOC_STREAMOFF, &type);
	if(ret < 0)
	{
		return MS_FAIL;
	}

	return unmmap();
}

int CameraV4L::GetImageData(unsigned char *data, int &len, int ms)
{
	int ret;

	fd_set fds;
	struct timeval tv;

	tv.tv_sec = ms/1000;
	tv.tv_usec = ms%1000;

	FD_ZERO (&fds);
	FD_SET (fd, &fds);

	ret = select (fd + 1, &fds, NULL, NULL, &tv);
	if (-1 == ret)
	{
		return MS_FAIL;
	}
	if (0 == ret)
	{
		return MS_TIMEOUT;
	}

	struct v4l2_buffer buf;
	memset(&buf, 0, sizeof(buf));
	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_MMAP;
	
	ret = xioctl(fd, VIDIOC_DQBUF, &buf);

	if(ret < 0)
	{
		return MS_FAIL;
	}

    assert (buf.index < REQ_BUF_LEN);

	memcpy(data, (char*)buffers[buf.index].start, len);

	ret = xioctl(fd, VIDIOC_QBUF, &buf);
	if(ret < 0)
	{
		return MS_FAIL;
	}

	return MS_SUCCESS;
}

}